#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     tLimits,        sensorI2CCustom9V)
#pragma config(Sensor, S4,     tIRBeacon,      sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          mTow,          tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          mLeftTow,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          mRightTow,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     dLeft1,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dLeft2,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     dRight1,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     dRight2,       tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_1,     aBallTip,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     aUExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_1,     aBallPickup,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     aLExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    sBallDrop,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    sIRSeeker,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "FTC_Toolkit.h"

// IR Beacon Read Position
//int Beacon_Locs[2] = {2500, 8000};
//int Turn_Points[3] = {2000, 5000, 8000};
//int First_Turn[3] = {DEG_180_TURN, DEG_90_TURN, DEG_45_TURN};

int IRVal = 0;

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
	motor[dLeft1] = motor[dLeft2] = motor[dRight1] = motor[dRight2] = 0;
  motor[aBallTip] = motor[aUExtend] = motor[aBallPickup] = motor[aLExtend] = 0;
  servo[sBallDrop] = 255;
  servo[sIRSeeker] = IR_SERVO_DOWN;
  ImplementVariables();
	wait1Msec(300);

  return;
}

task main()
{
	initializeRobot();
	waitForStart();
	StartTask(ReleaseBoom);
	waitForTask(&tReleaseBoom_Active);

	DriveSystem(75, 0);
	wait1Msec(5000);
	DriveSystem(0, 75);
	wait1Msec(2000);
}
